﻿// Different search strategies produce very different running times.
// Three strategies are supported here: depth first, breadth first,
// and best first. The latter appears to be the best strategy for
// random asymmetric Travelling Salesman problems.
// #define DEPTHFIRST
// #define BREADTHFIRST
#define BESTFIRST

using System;
using System.Collections.Generic;
using System.Threading;
using System.Threading.Tasks;

namespace Dwarf.Managed {

    class AssignmentProblem {
        private const int infinity = int.MaxValue;

        // Problem definition
        private int cityCount;                          // count of points
        private int[,] distances;                       // square matrix of distances between all points

        private int[] constraintMap;
        // constraintMap[i] >  0 -> (i, constraintMap[i]-1) is Included
        // constraintMap[i] == 0 -> no constraints on (i, ?)
        // constraintMap[i] <  0 -> -1 - constraintMap[i] = index of first element in exclusions list.
        private List<int> exclusions;
        // See constraintMap. List is terminated by -1.

        AssignmentProblem parent;
        int childIndex;

        private int[] patchedToCities;
        private int[] patchedFromCities;
        private int[] toCities;                     // final array of cities
        private int[] fromCities;                   // originating city
        private long lowerBound = infinity;         // sum of distances between cities
        private long value = infinity;              // sum of distances between cities

        // Dual variable associated with each row:
        int[] u;
        // Dual variable associated with each column:
        int[] v;

        public AssignmentProblem(int cityCount, int[,] distances) {
            this.parent = null;
            this.cityCount = cityCount;
            this.distances = distances;
            this.constraintMap = new int[cityCount];
            this.exclusions = new List<int>();
        }

        private AssignmentProblem(AssignmentProblem parent, int index) {
            this.parent = parent;
            this.lowerBound = parent.LowerBound;
            this.childIndex = index;
        }

        public long LowerBound { get { return lowerBound; } }
        public long Value { get { return value; } }
        public int[] GetCities() {
            return patchedToCities ?? toCities;
        }

        /// <summary>
        /// Solves the assignment problem and returns the total distance.
        /// </summary>
        public long Solve() {
            bool success;
            if (parent == null)
                success = SolveFromScratch();
            else {
                AddConstraints();
                success = SolveIncremental();
                parent = null;
            }

            if (!success)
                return infinity;

            // Compute total distance.
            this.lowerBound = 0;
            for (int i = 0; i < cityCount; i++)
                lowerBound += u[i] + v[i];
            // Extract cycles
            ComputeCycles();
            return lowerBound;
        }

        #region Constraints

        private bool IsAllowed(int row, int column) {
            int value = constraintMap[row];
            return (row != column) && ((value == 0) ? (row != column) : IsAllowedConstraint(value, column));
        }
        private bool IsAllowedConstraint(int value, int column) {
            if (value > 0)
                // Include
                return column == (value - 1);
            else {
                // Exclude
                value = -1 - value;
                while (exclusions[value] >= 0) {
                    if (exclusions[value] == column)
                        return false;
                    value++;
                }
                return true;
            }
        }

        private void Include(int from, int to) {
            this.constraintMap[from] = 1 + to;
        }
        private void Exclude(int from, int to) {
            int index = this.constraintMap[from];
            if (index == 0) {
                // No constraints yet. Add this one at the end.
                this.constraintMap[from] = -1 - this.exclusions.Count;
                this.exclusions.Add(to);
                this.exclusions.Add(-1);
            } else if (this.constraintMap[from] < 0) {
                // We have a constraint. Just insert this one at the front.
                for (int i = -1 - index; this.exclusions[i] != -1; i++) {
                    if (this.exclusions[i] == to) {
                        //Console.WriteLine("Oops! This shouldn't happen!");
                        return;
                    }
                }
                this.exclusions.Insert(-1 - index, to);
                for (int i = 0; i < this.constraintMap.Length; i++) {
                    if (i == from)
                        continue;
                    if (this.constraintMap[i] < index)
                        this.constraintMap[i]--;
                }
            }
        }

        private void AddConstraints() {
            this.cityCount = parent.cityCount;
            this.distances = parent.distances;

            this.constraintMap = (int[])parent.constraintMap.Clone();

            // Add inclusions
            for (int i = 0; i < this.childIndex; i++)
                Include(parent.childFromCities[i], parent.childToCities[i]);

            // Add exclusions
            this.exclusions = new List<int>(parent.exclusions.Count + 4);
            this.exclusions.AddRange(parent.exclusions);
            Exclude(parent.childFromCities[this.childIndex], parent.childToCities[this.childIndex]);
            if (this.childIndex > 0) {
                Exclude(parent.toCities[this.childIndex - 1], parent.fromCities[0]);
            }
        }

        #endregion

        private bool SolveFromScratch() {
            this.toCities = new int[cityCount];
            this.fromCities = new int[cityCount];
            this.u = new int[cityCount];
            this.v = new int[cityCount];

            // Label all cities as unassigned
            int assignedCities = 0;
            for (int i = 0; i < cityCount; i++) {
                toCities[i] = -1;
                fromCities[i] = -1;
            }

            // Find the smallest distance in each column.
            for (int column = 0; column < cityCount; column++) {
                // Find the i distance in the column.
                int minDistance = infinity;
                int minRow = -1;
                for (int row = 0; row < cityCount; row++) {
                    if (!IsAllowed(row, column))
                        continue;
                    int distance = distances[row, column];
                    if (distance < minDistance || (distance == minDistance && toCities[row] < 0)) {
                        minRow = row;
                        minDistance = distance;
                    }
                }
                if (minRow < 0)
                    return false;

                // This distance is the value of the dual variable of the column.
                v[column] = minDistance;

                // If the row is unassigned, we can assign it.
                if (toCities[minRow] < 0) {
                    // Assign column to row minRow
                    assignedCities++;
                    fromCities[column] = minRow;
                    toCities[minRow] = column;
                    u[minRow] = 0;
                }
            }

            // Now look through unassigned rows, 
            // and see if they can be assigned.
            for (int row = 0; row < cityCount; row++) {
                if (toCities[row] >= 0)
                    continue;

                int minReducedCost = infinity;
                int minColumn = -1;
                for (int column = 0; column < cityCount; column++) {
                    if (!IsAllowed(row, column))
                        continue;
                    int reducedCost = distances[row, column] - v[column];
                    if (reducedCost < minReducedCost
                        || (reducedCost == minReducedCost && fromCities[column] < 0 && fromCities[minColumn] >= 0)) {
                        minReducedCost = reducedCost;
                        minColumn = column;
                    }
                }

                if (minColumn < 0)
                    return false;

                u[row] = minReducedCost;

                if (fromCities[minColumn] < 0) {
                    // Assign minColumn to row i
                    assignedCities++;
                    toCities[row] = minColumn;
                    fromCities[minColumn] = row;
                }
            }

            if (assignedCities < cityCount) {
                AllocateWorkspace();
                for (int row = 0; row < cityCount; row++) {
                    if (toCities[row] >= 0)
                        continue;
                    FindAugmentingPath(row);
                }
                DeallocateWorkspace();
            }
            return true;
        }


        private bool SolveIncremental() {
            this.toCities = (int[])parent.toCities.Clone();
            this.fromCities = (int[])parent.fromCities.Clone();
            this.u = (int[])parent.u.Clone();
            this.v = (int[])parent.v.Clone();

            // Remove excluded edge.
            int row = parent.childFromCities[this.childIndex];
            int column = toCities[row];
            fromCities[column] = -1;
            toCities[row] = -1;

            AllocateWorkspace();
            FindAugmentingPath(row);
            DeallocateWorkspace();
            return true;
        }

        int[] pathOrigins;
        int[] reducedCosts;
        int[] labeledRow;
        int[] unlabeledColumn;

        void AllocateWorkspace() {
            pathOrigins = new int[cityCount];
            reducedCosts = new int[cityCount];
            labeledRow = new int[cityCount];
            unlabeledColumn = new int[cityCount];
        }
        void DeallocateWorkspace() {
            pathOrigins = null;
            reducedCosts = null;
            labeledRow = null;
            unlabeledColumn = null;
        }
        void FindAugmentingPath(int row) {

            labeledRow[0] = row;
            for (int column = 0; column < cityCount; column++) {
                if (!IsAllowed(row, column))
                    reducedCosts[column] = infinity;
                else
                    reducedCosts[column] = distances[row, column] - u[row] - v[column];
                pathOrigins[column] = row;
                unlabeledColumn[column] = column;
            }
            int unlabeledColumnCount = cityCount;
            int labeledRowCount = 1;

            for (; ; ) {
                // Find a zero in an unlabeled column
                bool foundZero = false;
                for (int l = 0; l < unlabeledColumnCount; l++) {
                    int column = unlabeledColumn[l];
                    if (reducedCosts[column] == 0) {
                        foundZero = true;

                        // If the column is unassigned, we're done.
                        if (fromCities[column] < 0) {
                            do {
                                int origin = pathOrigins[column];
                                fromCities[column] = origin;
                                int hold = toCities[origin];
                                toCities[origin] = column;
                                column = hold;
                            } while (column >= 0);
                            return;
                        }

                        // label the row corresponding to the column.
                        labeledRow[labeledRowCount++] = fromCities[column];
                        // remove label from column column
                        unlabeledColumn[l] = unlabeledColumn[--unlabeledColumnCount];
                        break;
                    }
                }
                if (foundZero) {
                    // Scan labeled rows
                    int r = labeledRow[labeledRowCount - 1];
                    for (int l = 0; l < unlabeledColumnCount; l++) {
                        int column = unlabeledColumn[l];
                        if (!IsAllowed(r, column))
                            continue;
                        int reducedCost = distances[r, column] - u[r] - v[column];
                        if (reducedCost < reducedCosts[column]) {
                            reducedCosts[column] = reducedCost;
                            pathOrigins[column] = r;
                        }
                    }
                } else {
                    // Update the dual variables
                    // Find minimum value.
                    int minReducedCost = infinity;
                    for (int l = 0; l < unlabeledColumnCount; l++) {
                        int column = unlabeledColumn[l];
                        if (reducedCosts[column] < minReducedCost)
                            minReducedCost = reducedCosts[column];
                    }
                    // Add minimum to dual variables of labeled rows
                    for (int l = 0; l < labeledRowCount; l++) {
                        int r = labeledRow[l];
                        u[r] += minReducedCost;
                    }
                    // Subtract minimum from dual variables of labeled columns
                    for (int column = 0; column < cityCount; column++) {
                        if (reducedCosts[column] == 0)
                            v[column] -= minReducedCost;
                        else
                            reducedCosts[column] -= minReducedCost;
                    }
                }

            }
        }

        #region Patching/merging of cycles
        struct Cycle {
            public int StartNode;   // First city in the cycle
            public int Length;      // Total # cities
            public int FreeLength;  // # cities not in the Include list
        }
        List<Cycle> cycles;
        int minCycle;

        /// <summary>
        /// Gets the starting point of the cycle of i length.
        /// </summary>
        /// <returns></returns>
        private void ComputeCycles() {
            bool[] visited = new bool[cityCount];
            this.cycles = new List<Cycle>();
            int cycleCount = 0;
            for (int i = 0; i < visited.Length; i++) {
                if (visited[i])
                    continue;
                cycleCount++;
                visited[i] = true;
                int next = toCities[i];
                int longest = i;
                int longestDistance = distances[i, next];
                int length = 1;
                int freeLength = constraintMap[i] > 0 ? 0 : 1;
                while (next != i) {
                    visited[next] = true;
                    int last = next;
                    next = toCities[next];
                    length++;
                    if (constraintMap[last] <= 0) {
                        freeLength++;
                        int distance = distances[last, next];
                        if (distance > longestDistance) {
                            longestDistance = distance;
                            longest = toCities[next];
                        }
                    }
                }
                Cycle cycle = new Cycle();
                cycle.Length = length;
                cycle.FreeLength = freeLength;
                cycle.StartNode = longest;
                this.cycles.Add(cycle);
            }

            if (cycles.Count == 1)
                value = lowerBound;
            else {
                this.cycles.Sort((x, y) => x.Length > y.Length ? +1 : x.Length < y.Length ? -1 : 0);
                int minFreeLength = int.MaxValue;
                for (int i = 0; i < this.cycles.Count; i++)
                    if (cycles[i].FreeLength < minFreeLength) {
                        minCycle = i;
                        minFreeLength = cycles[i].FreeLength;
                    }
            }
        }

        public int CycleCount { get { return cycles.Count; } }
        public int ChildCount { get { return cycles[minCycle].FreeLength; } }

        private void Patch(int from, int to, int from2, int to2) {
            patchedToCities[from] = to2;
            patchedFromCities[to2] = from;
            patchedToCities[from2] = to;
            patchedFromCities[to] = from2;
        }

        /// <summary>
        /// Patches the cycles with the specified start cities using the
        /// Karp-Steele patch algorithm.
        /// </summary>
        /// <param name="startCity"></param>
        /// <param name="startCity2"></param>
        /// <returns>The cost of the patch.</returns>
        private long Patch(int startCity, int startCity2) {
            int bestLast = -1;
            int bestLast2 = -1;
            int last = startCity;
            int next = toCities[last];
            long minPenalty = long.MaxValue;
            while (next != startCity) {

                long basePenalty = -distances[last, next];
                int last2 = startCity2;
                int next2 = toCities[last2];
                while (next2 != startCity2) {
                    if (IsAllowed(last, next2) && IsAllowed(last2, next)) {
                        long penalty = basePenalty - distances[last2, next2]
                            + distances[last, next2] + distances[last2, next];
                        if (penalty < minPenalty) {
                            if (penalty == 0) {
                                Patch(last, next, last2, next2);
                                return 0;
                            }
                            minPenalty = penalty;
                            bestLast = last;
                            bestLast2 = last2;
                        }
                    }
                    last2 = next2;
                    next2 = toCities[last2];
                }
                last = next;
                next = toCities[last];
            }

            if (bestLast < 0 || bestLast2 < 0)
                return infinity;
            Patch(bestLast, patchedToCities[bestLast], bestLast2, patchedToCities[bestLast2]);
            return minPenalty;
        }

        public long Patch(long upperBound) {
            patchedToCities = (int[])toCities.Clone();
            patchedFromCities = (int[])fromCities.Clone();

            long maxPenalty = upperBound - lowerBound;
            long penalty = 0;
            for (int cycle = cycles.Count - 1; cycle > 0; --cycle) {
                int startCity = cycles[cycle].StartNode;
                int startCity2 = cycles[cycle - 1].StartNode;
                penalty += Patch(startCity, startCity2);
                if (penalty >= maxPenalty)
                    break;
            }
            if (penalty >= maxPenalty) {
                // We can discard these if we don't have a better result.
                patchedFromCities = null;
                patchedToCities = null;
            }
            this.value = penalty == infinity ? infinity : lowerBound + penalty;
            return this.value;
        }
        #endregion

        #region Branching
        public static int Comparer(AssignmentProblem x, AssignmentProblem y) {
            if (x.LowerBound < y.LowerBound)
                return 1;
            if (x.LowerBound > y.LowerBound)
                return -1;
            if (x.ChildCount < y.ChildCount)
                return 1;
            if (x.ChildCount > y.ChildCount)
                return -1;
            return 0;
        }

        private int[] childFromCities;
        private int[] childToCities;

        public void GenerateSubproblems(List<AssignmentProblem> problemList) {
            if (cycles.Count == 0)
                return;

            int minLength = cycles[minCycle].FreeLength;
            childFromCities = new int[minLength];
            childToCities = new int[minLength];

            int last = cycles[minCycle].StartNode;
            int next;
            int index = 0;
            do {
                next = toCities[last];
                if (constraintMap[last] <= 0) {
                    childFromCities[index] = last;
                    childToCities[index++] = next;
                }
 
                last = next;
            } while (last != cycles[minCycle].StartNode);

            lock (problemList) {
                problemList.Capacity = Math.Max(problemList.Capacity, problemList.Count + minLength);
                for (int i = 0; i < minLength; i++) {
                    //problemList.Add(new AssignmentProblem(this, (minLength - 1) - i));
                    problemList.Add(new AssignmentProblem(this, i));
                }
            }
        }
        #endregion
    }

    /// <summary>Solve the Traveller problem using 'Branch And Bound' method without parallelization</summary>
    /// 
    class Solver {
        private Configurator dwarfConfigurator;
        private int cityCount;                          // count of points
        private int[,] distances;                       // square matrix of distances between all points
        private long totalNodes;                        // total # nodes visited

        /// <summary>Construct the new solver</summary>
        /// <param name="dwarfConfigurator">Processed command line arguments</param>
        /// 
        public Solver(ref Configurator dwarfConfigurator) {
            this.dwarfConfigurator = dwarfConfigurator;
            dwarfConfigurator.WriteSettings();                      // write settings to console and profile file
            dwarfConfigurator.GetData(ref cityCount, ref distances);// getting the array of edge values
        }

        private const int infinity = int.MaxValue;
        private long upperBound;
        private long bestLowerBound;
        private List<AssignmentProblem> problemList = new List<AssignmentProblem>();
        private List<AssignmentProblem> solvedList = new List<AssignmentProblem>();

        private AssignmentProblem best;

        /// <summary>
        /// Adds a solved problem to the solved list, keeping the solved list sorted.
        /// </summary>
        private void AddSolvedProblem(AssignmentProblem problem) {
            lock (solvedList) {
#if DEPTHFIRST
                solvedList.Add(problem);
#endif
#if BREADTHFIRST
                solvedList.Insert(0, problem);
#endif
#if BESTFIRST
                int index = solvedList.BinarySearch(problem, Comparer.Instance);
                if (index < 0)
                    solvedList.Insert(~index, problem);
                else
                    solvedList.Insert(index, problem);
#endif
            }
        }
        class Comparer : IComparer<AssignmentProblem> {

            public static readonly Comparer Instance = new Comparer();
            #region IComparer<AssignmentProblem> Members

            public int Compare(AssignmentProblem x, AssignmentProblem y) {
                return AssignmentProblem.Comparer(x, y);
            }

            #endregion
        }

        public IEnumerable<AssignmentProblem> Assignments() {
            for (; ; ) {
                for (int i = 0; i < problemList.Count; i++) {
                    AssignmentProblem problem = problemList[i];
                    bestLowerBound = Math.Max(bestLowerBound, problem.LowerBound);
                    yield return problem;
                }
                lock (problemList) {
                    problemList.Clear();
                }

                if (solvedList.Count > 0) {
                    AssignmentProblem problem = null;
                    lock (solvedList) {
#if BESTFIRST
                        {
                            // Prune solved problems
                            int index = 0;
                            while (index < solvedList.Count && solvedList[index].LowerBound >= upperBound) {
                                index++;
                            }
                            if (index > 0)
                                solvedList.RemoveRange(0, index);
                        }
#endif
                        while (solvedList.Count > 0) {
                            int index = solvedList.Count - 1;
                            problem = solvedList[index];
                            solvedList.RemoveAt(index);
                            if (problem.LowerBound < upperBound)
                                break;
                            problem = null;
                        }
                    }
                    if (problem == null)
                        break;
                    problem.GenerateSubproblems(problemList);
                } else
                    break;
            }
        }

        int[] bestCities;
        private void SetBestProblem(AssignmentProblem problem) {
            best = problem;
            upperBound = best.Value;
            bestCities = best.GetCities();
            Console.WriteLine("Shortest distance: {0}, {1} nodes visited.", upperBound, totalNodes);
        }

        /// <summary>Tast Parallel Library method of solving</summary>
        ///
        public void Solve() {
            object lockObject = new object();
            best = new AssignmentProblem(cityCount, distances);
            bestLowerBound = best.Solve();
            if (best.CycleCount == 1)
                upperBound = best.LowerBound;
            else {
                upperBound = best.Patch(infinity);
                best.GenerateSubproblems(problemList);
            }
            totalNodes = 1;
            SetBestProblem(best);

            for (; ; ) {
                Parallel.ForEach<AssignmentProblem>(Assignments(), problem => {
                    if (problem.LowerBound >= upperBound)
                        return;
                    long lowerBound = problem.Solve();
                    Interlocked.Increment(ref totalNodes);
                    if (problem.CycleCount == 1) {
                        if (lowerBound < upperBound) {
                            lock (lockObject) {
                                if (lowerBound < upperBound)
                                    SetBestProblem(problem);
                            }
                        }
                    } else if (lowerBound >= upperBound) {
                        return;
                    } else {
                        long newUpperBound = problem.Patch(upperBound);
                        if (newUpperBound < upperBound) {
                            lock (lockObject) {
                                if (newUpperBound < upperBound) {
                                    SetBestProblem(problem);
                                }
                            }
                        }
                        AddSolvedProblem(problem);
                    }
                });
                if (problemList.Count == 0 && solvedList.Count == 0)
                    break;
            }
        }

        /// <summary>Problem results output</summary>
        /// <param name="time">Spent time</param>
        ///
        public void Finish(double time) {
            dwarfConfigurator.Close(time, bestCities, cityCount);
            Console.WriteLine("Total Distance    : " + this.upperBound);
            Console.WriteLine("Total # nodes     : " + this.totalNodes);
        }
    }
}